﻿#region License
/**	
The MIT License (MIT)

Copyright (c) 2013 Philipp Schillinger, Michael Alekseew, Andreas Braun, Steeven Zeiß

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE. 
 **/
#endregion

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;

namespace IndoorLocalization {

	/// <summary>
	/// Receives rays from sensors and combines them to find valid entities.
	/// This class implements a local merger that runs an update thread and
	/// is really merging all rays it receives.
	/// </summary>
	[Serializable()]
	public class LocalMerger : SensorMerger {

		/// <summary>
        /// Thread that is used to run the merge method.
		/// </summary>
		[NonSerialized]
        private Thread mergerThread;
        /// <summary>
        /// The rate of how many merges are done per second.
        /// Should not be smaller than the update rate of the sensors
        /// because this would be inefficient.
        /// </summary>
        private int mergeRate = 30;

        /// <summary>
        /// Creates a new merger for a bunch of sensors.
        /// Each sensor that should be merged by this merger has to specify this merger
        /// as its one.
        /// When created, the merger immediately starts updating and searching for entities
        /// by itself.
		/// </summary>
		/// <param name="name">Unique name for this merger to be displayed.</param>
        public LocalMerger(string name) {
			this.name = name;

			StartMerger();
        }

		/// <summary>
		/// Initializes the important attributes and starts the merge thread.
		/// </summary>
		protected override void StartMerger() {
			// initialize attributes
			rays = new List<SensorRay>();
			entities = new List<Entity>();

			// start merge thread
			mergerThread = new Thread(Merge);
			mergerThread.IsBackground = true;
			mergerThread.Start();
		}

		/// <summary>
		/// Indicates whether the merge thread of this merger is running.
		/// </summary>
		public override bool IsRunning() {
			return mergerThread != null;
		}

		/// <summary>
		/// Indicates whether this merger is local or remote.
		/// </summary>
		public override bool IsLocal() {
			return true;
		}

        /// <summary>
        /// Endless loop to merge found rays for the estimation of entities.
        /// Should be started as a background thread.
        /// </summary>
        private void Merge() {
            while (true) {
				List<Entity> foundEntities = new List<Entity>();

                // do not merge if not more than one ray was found
				if (rays.Count > 1) {

                    lock (rays) {
                        // determine entities
                        for (int i = 0; i < rays.Count - 1; i++) {
                            for (int j = i + 1; j < rays.Count; j++) {
                                if (rays[i].DoesIntersect(rays[j])) {
                                    Entity e = new Entity(rays[i], rays[j]);
                                    foundEntities.Add(e);
                                }
                            }
                        }
                        // combine entities that seem to represent the same
                        for (int i = 0; i < foundEntities.Count - 1; i++) {
                            for (int j = i + 1; j < foundEntities.Count; j++) {
                                if (foundEntities[i].IsNear(foundEntities[j])) {
                                    if (foundEntities[i].Reliability > foundEntities[j].Reliability) {
                                        foundEntities[i].Combine(foundEntities[j]);
                                        foundEntities.Remove(foundEntities[j--]);
                                    } else {
                                        foundEntities[j].Combine(foundEntities[i]);
                                        foundEntities.Remove(foundEntities[i--]);
                                        break;
                                    }
                                }
                            }
                        }
                        // filter entities that intersect invalidly
						List<Entity> entityPool = new List<Entity>(foundEntities);
						Dictionary<SensorRay, List<Entity>> entityLists = new Dictionary<SensorRay, List<Entity>>();
							// collect entities regarding to their rays
                        for (int i = 0; i < rays.Count; i++) {
							entityLists.Add(rays[i], foundEntities.FindAll(new Predicate<Entity>(target => {
								return target.Rays.Contains(rays[i]);
							})));
						}
							// sort rays so the rays with few entities come first (the less entities, the more reliable they are)
						rays.Sort(new Comparison<SensorRay>((ray1, ray2) => {
							return entityLists[ray1].Count - entityLists[ray2].Count;
						}));
							// only keep one entity per ray, the one which fits best to the estimated distance
							// if one ray intersects with two rays, it can have two entities because the other two rays chose them
						for (int i = 0; i < rays.Count; i++) {
                            HCoord origin = rays[i].Origin;
							// sort entities depending on their distance
							entityLists[rays[i]].Sort(new Comparison<Entity>((entity1, entity2) => {
								return (int)(origin.DistanceTo(entity1.Position) - origin.DistanceTo(entity2.Position));
							}));
							// project the estimated distance indicator onto the array of found entities
							int estimatedPosition = Math.Min((int)Math.Round(rays[i].EstimatedDistanceIndicator / 10.0f * entityLists[rays[i]].Count), entityLists[rays[i]].Count - 1);
							// remove all entities that do not fit and were not chosen by other rays before
							for (int e = 0; e < entityLists[rays[i]].Count; e++) {
								Entity entity = entityLists[rays[i]][e];
								if (e != estimatedPosition && entityPool.Contains(entity)) {
									foundEntities.Remove(entity);
								}
								if (e == estimatedPosition && entityPool.Contains(entity))
									entityPool.Remove(entity);
							}
                        }
                    }
					// determine which entity could represent an already known one
					List<Entity> oldEntities = GetEntitiesTS();
					for (int i = 0; i < foundEntities.Count; i++) {
						for (int j = 0; j < oldEntities.Count; j++) {
							if (foundEntities[i].IsNextOf(oldEntities[j])) {
								foundEntities[i].Sequel(oldEntities[j]);
								foundEntities[i].IncreaseReliability();
								oldEntities.Remove(oldEntities[j--]);
							}
						}
					}
					// guess lost but reliable entities
					for (int i = 0; i < oldEntities.Count; i++) {
						if (oldEntities[i].Reliability > 1) {
							Entity guessed = new Entity(oldEntities[i]);
							guessed.DecreaseReliability();
							foundEntities.Add(guessed);
						}
					}

				} else {
					// guess lost but reliable entities
					List<Entity> oldEntities = GetEntitiesTS();
					for (int i = 0; i < oldEntities.Count; i++) {
						if (oldEntities[i].Reliability > 1) {
							Entity guessed = new Entity(oldEntities[i]);
							guessed.DecreaseReliability();
							foundEntities.Add(guessed);
						}
					}
				}

				// update entity list
				lock (entities) {
					entities = foundEntities;
				}
                Thread.Sleep(1000 / mergeRate);
            }
        }
	}

}
